/*
 *        (C) COPYRIGHT Ingenic Limited.
 *             ALL RIGHTS RESERVED
 *
 * File       : lib_carto_contour.cpp
 * Authors    : ycai@ubuntu
 * Create Time: 2022-06-09:01:54:33
 * Description:
 * 
 */

#include <iostream>
#include "string.h"
#include "yaml-cpp/yaml.h"
#include "lib_carto_contour.h"


// The >> operator disappeared in yaml-cpp 0.5, so this function is
// added to provide support for code written under the yaml-cpp 0.3 API.
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
  i = node.as<T>();
}

using namespace std;

carto_map_type::carto_map_type(const char* yaml_p,const char* pgm_p)
{
  //TODO:将分辨率等信息同一到一张地图上，完善地图信息读取程序!!!
  //const char *tmp = "hdmap.pgm";
  //const char *tmp1 = "hdmap.yaml";
  const char *tmp = pgm_p;
  const char *tmp1 = yaml_p; 

  map_ = (map_t*) malloc(sizeof(map_t));

  // Assume we start at (0, 0)
  map_->origin_x = 0;
  map_->origin_y = 0;
  
  // Make the size odd
  map_->size_x = 0;
  map_->size_y = 0;
  map_->scale = 0;

  int negate;

  map_load_yaml(tmp1, &map_->scale, &negate, &map_->origin_x, &map_->origin_y);

  // Allocate storage for main map
  map_->cells = (map_cell_t*) NULL;

  int readmapflag = map_load_occ(map_, tmp, map_->scale/*resolution*/, negate/*negate*/);
  
  
  if(readmapflag == 1)
    {
      printf("Reading carto grid map success!\n");
    }
  else
    {
      printf("Reading carto grid map failed!\n");
    }

  map_->origin_x = map_->origin_x + (map_->size_x / 2) * map_->scale;
  map_->origin_y = map_->origin_y + (map_->size_y / 2) * map_->scale;
}

carto_map_type::~carto_map_type()
{
  //Destroy map
  free(map_->cells);
  free(map_);
}

int carto_map_type::map_load_yaml(const char *filename, double *scale, int *negate,double *origin_x, double *origin_y)
{
  std::ifstream fin(filename);
  if(fin.fail())
    {
      printf("Could not open %s.\n",filename);
      return -1;
    }
  YAML::Node doc = YAML::Load(fin);
  try
    {
      doc["resolution"] >> (*scale);      
    }
  catch(YAML::InvalidScalar &)
    {
      printf("The map does not contain a resolution tag or it is invalid.\n");
    }
  try
    {
      doc["negate"] >> *negate;      
    }
  catch(YAML::InvalidScalar &)
    {
      printf("The map does not contain a negate tag or it is invalid.\n");
      return -1;
    }
  try {
    doc["origin"][0] >> *origin_x;
    doc["origin"][1] >> *origin_y;    
  } catch (YAML::InvalidScalar &) {
    printf("The map does not contain an origin tag or it is invalid.\n");
    return -1;
  }
  //printf("resolution:%f,negate:%d,origin_x:%f,origin_y:%f\n",*scale,*negate,*origin_x,*origin_y);
  return 1;
}

// Load an occupancy grid
int carto_map_type::map_load_occ(map_t *map, const char *filename, double scale, int negate)
{
    FILE *file;
    char magic[3];
    int i, j;
    int ch, occ;
    int width, height, depth;
    map_cell_t *cell;

    // Open file
    file = fopen(filename, "r");
    if (file == NULL) {
        fprintf(stderr, "%s: %s\n", strerror(errno), filename);
        return -1;
    }

    // Read ppm header

    if ((fscanf(file, "%2s \n", magic) != 1) || (strcmp(magic, "P5") != 0)) {
        fprintf(stderr, "incorrect image format; must be PGM/binary");
        fclose(file);
        return -1;
    }

    // Ignore comments
    while ((ch = fgetc(file)) == '#')
        while (fgetc(file) != '\n');
    ungetc(ch, file);

    // Read image dimensions
    if (fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3) {
        fprintf(stderr, "Failed to read image dimensions");
        return -1;
    }

    // Allocate space in the map
    if (map->cells == NULL) {
        map->scale = scale;
        map->size_x = width;
        map->size_y = height;
        map->cells = (map_cell_t *) calloc(width * height, sizeof(map->cells[0]));
    } else {
        if (width != map->size_x || height != map->size_y) {
            //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
            return -1;
        }
    }

    // Read in the image
    for (j = height - 1; j >= 0; j--) {
        for (i = 0; i < width; i++) {
            ch = fgetc(file);

            // Black-on-white images
            if (!negate) {
                if (ch < depth / 4)//defalut:1/4
                    occ = +1;//被占据
                else if (ch > 3 * depth / 4)
                    occ = -1;//空闲
                else
                    occ = -1;//占据状态未知,如果是二值图的话，该值改为-1
            }

                // White-on-black images
            else {
                if (ch < depth / 4)
                    occ = -1;
                else if (ch > 3 * depth / 4)
                    occ = +1;
                else
                    occ = 0;
            }

            if (!MAP_VALID(map, i, j))
                continue;
//      cell = map->cells + MAP_INDEX(map, i, j);
//      cell->occ_state = occ;
            map->cells[MAP_INDEX(map, i, j)].occ_state = occ;
        }
    }

    fclose(file);

    return 1;
}


/*
double cartomap_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
{
  // Bresenham raytracing
  int x0, x1, y0, y1;
  int x, y;
  int xstep, ystep;
  char steep;
  int tmp;
  int deltax, deltay, error, deltaerr;

  x0 = MAP_GXWX(map, ox);
  y0 = MAP_GYWY(map, oy);

  x1 = MAP_GXWX(map, ox + max_range * cos(oa));
  y1 = MAP_GYWY(map, oy + max_range * sin(oa));

  if (abs(y1 - y0) > abs(x1 - x0))
    steep = 1;
  else
    steep = 0;

  if (steep) {
    tmp = x0;
    x0 = y0;
    y0 = tmp;

    tmp = x1;
    x1 = y1;
    y1 = tmp;
  }

  deltax = abs(x1 - x0);
  deltay = abs(y1 - y0);
  error = 0;
  deltaerr = deltay;

  x = x0;
  y = y0;

  if (x0 < x1)
    xstep = 1;
  else
    xstep = -1;
  if (y0 < y1)
    ystep = 1;
  else
    ystep = -1;

  if (steep) {
    if (!MAP_VALID(map, y, x) || map->cells[MAP_INDEX(map, y, x)].occ_state > -1)
      return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
  } else {
    if (!MAP_VALID(map, x, y) || map->cells[MAP_INDEX(map, x, y)].occ_state > -1)
      return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
  }

  while (x != (x1 + xstep * 1)) {
    x += xstep;
    error += deltaerr;
    if (2 * error >= deltax) {
      y += ystep;
      error -= deltax;
    }

    if (steep) {
      if (!MAP_VALID(map, y, x) || map->cells[MAP_INDEX(map, y, x)].occ_state > -1)
	return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
    } else {
      if (!MAP_VALID(map, x, y) || map->cells[MAP_INDEX(map, x, y)].occ_state > -1)
	return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
    }
  }
  return 150.0;
}
*/


double cartomap_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
{
  // Bresenham raytracing
  int x0, x1, y0, y1;
  int x, y;
  int xstep, ystep;
  char steep;
  int tmp;
  int deltax, deltay, error, deltaerr;

  x0 = MAP_GXWX(map, ox);
  y0 = MAP_GYWY(map, oy);

  x1 = MAP_GXWX(map, ox + max_range * cos(oa));
  y1 = MAP_GYWY(map, oy + max_range * sin(oa));

  if (abs(y1 - y0) > abs(x1 - x0))
    steep = 1;
  else
    steep = 0;

  if (steep)
    {
      tmp = x0;
      x0 = y0;
      y0 = tmp;

      tmp = x1;
      x1 = y1;
      y1 = tmp;
    }

  deltax = abs(x1 - x0);
  deltay = abs(y1 - y0);
  error = 0;
  deltaerr = deltay;

  x = x0;
  y = y0;

  if (x0 < x1)
    xstep = 1;
  else
    xstep = -1;
  if (y0 < y1)
    ystep = 1;
  else
    ystep = -1;

  if (steep)
    {
      if (MAP_VALID(map, y, x) && (map->cells[MAP_INDEX(map, y, x)].occ_state == 1))
	return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
      else if(!MAP_VALID(map, y, x))
	return 0.0;
    }
  else
    {
      if (MAP_VALID(map, x, y) && (map->cells[MAP_INDEX(map, x, y)].occ_state == 1))
	return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
      else if(!MAP_VALID(map, x, y))
	return 0.0;
    }

  while (x != (x1 + xstep * 1))
    {
      x += xstep;
      error += deltaerr;
      if (2 * error >= deltax)
	{
	  y += ystep;
	  error -= deltax;
	}

      if (steep)
	{
	  if (MAP_VALID(map, y, x) && (map->cells[MAP_INDEX(map, y, x)].occ_state == 1))
	    return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
	  else if(!MAP_VALID(map, y, x))
	    return 0.0;
	}
      else
	{
	  if (MAP_VALID(map, x, y) && (map->cells[MAP_INDEX(map, x, y)].occ_state == 1))
	    return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
	  else if(!MAP_VALID(map, x, y))
	    return 0.0;
	}
    }
  return 0.0;
}


void extract_scan_from_cartomap(const carto_map_type* contour_map,point_cloud_raw_type* point_cloud_map_raw,const pose_type* laser_scanner_pose,double max_range)
{
  for (int i = 0; i < point_cloud_map_raw->num_points_per_scan; ++i)
    {
      double angle = laser_scanner_pose->theta + i * 2.0 * M_PI / point_cloud_map_raw->num_points_per_scan - M_PI;//from [-PI to PI)
      //angle = atan2(sin(angle), cos(angle));
      point_cloud_map_raw->range[i] = cartomap_calc_range(contour_map->map_,laser_scanner_pose->x,laser_scanner_pose->y,angle,max_range);
      point_cloud_map_raw->echo[i] = 0;      
    }
}


